use crate::{Vec3, Mat4, Mat3};
use std::ops::{Mul, Div, MulAssign};

#[derive(Copy, Clone)]
pub struct Quat {
    pub w: f32,
    pub v: Vec3,
}

impl Quat {
    pub fn new(w: f32, v: Vec3) -> Self {
        Self { w, v }
    }

    pub fn euler(yaw: f32, pitch: f32, roll: f32) -> Self {
        let half_yaw = yaw / 2.0;
        let half_pitch = pitch / 2.0;
        let half_roll = roll / 2.0;

        let sin_hy = f32::sin(half_yaw);
        let cos_hy = f32::cos(half_yaw);

        let sin_hp = f32::sin(half_pitch);
        let cos_hp = f32::cos(half_pitch);

        let sin_hr = f32::sin(half_roll);
        let cos_hr = f32::cos(half_roll);

        let w = cos_hy * sin_hp * cos_hr + sin_hy * cos_hp * sin_hr;
        let x = sin_hy * cos_hp * cos_hr - cos_hy * sin_hp * sin_hr;
        let y = -sin_hy * sin_hp * cos_hr + cos_hy * cos_hp * sin_hr;
        let z = cos_hy * cos_hp * cos_hr + sin_hy * sin_hp * sin_hr;

        Self {
            w,
            v: Vec3(x, y, z)
        }
    }

    pub fn from_coord(w: f32, x: f32, y: f32, z: f32) -> Self {
        Self {
            w,
            v: Vec3(x, y, z),
        }
    }

    pub fn from_angle_axis(rad: f32, axis: Vec3) -> Self {
        Self {
            w: f32::cos(rad),
            v: f32::sin(rad) * axis,
        }
    }

    pub fn conj(&self) -> Self {
        Self {
            w: self.w,
            v: -self.v
        }
    }

    pub fn inv(&self) -> Self {
        self.conj() / self.magnitude()
    }

    pub fn dot(self, rhs: Self) -> f32 {
        self.w * rhs.w + Vec3::dot(self.v, rhs.v)
    }

    pub fn magnitude(&self) -> f32 {
        f32::sqrt(self.dot(*self))
    }

    pub fn into_mat3(self) -> Mat3 {
        self.into()
    }

    pub fn into_mat4(self) -> Mat4 {
        self.into()
    }
}

impl Into<Mat3> for Quat {
    fn into(self) -> Mat3 {
        let dwx = 2.0 * self.w * self.v.0;
        let dwy = 2.0 * self.w * self.v.1;
        let dwz = 2.0 * self.w * self.v.2;
        let dxx = 2.0 * self.v.0 * self.v.0;
        let dxy = 2.0 * self.v.0 * self.v.1;
        let dxz = 2.0 * self.v.0 * self.v.2;
        let dyy = 2.0 * self.v.1 * self.v.1;
        let dyz = 2.0 * self.v.1 * self.v.2;
        let dzz = 2.0 * self.v.2 * self.v.2;

        Mat3 {
            m: [
                [1.0 - dyy - dzz, dxy - dwz, dxz + dwy],
                [dxy + dwz, 1.0 - dxx - dzz, dyz - dwx],
                [dxz - dwy, dyz + dwx, 1.0 - dxx - dyy],
            ]
        }
    }
}

impl Into<Mat4> for Quat {
    fn into(self) -> Mat4 {
        self.into_mat3().into()
    }
}

impl From<Vec3> for Quat {
    fn from(v: Vec3) -> Self {
        Self::new(0.0, v)
    }
}

impl Mul for Quat {
    type Output = Self;

    fn mul(self, rhs: Self) -> Self::Output {
        Self::new(self.w * rhs.w - Vec3::dot(self.v, rhs.v), self.w * rhs.v + rhs.w * self.v + Vec3::cross(self.v, rhs.v))
    }
}

impl MulAssign for Quat {
    fn mul_assign(&mut self, rhs: Self) {
        self.w = self.w * rhs.w - Vec3::dot(self.v, rhs.v);
        self.v = self.w * rhs.v + rhs.w * self.v + Vec3::cross(self.v, rhs.v);
    }
}

impl Div<f32> for Quat {
    type Output = Quat;

    fn div(self, rhs: f32) -> Self::Output {
        Self {
            w: self.w / rhs,
            v: self.v / rhs
        }
    }
}